
#include "ros/ros.h"
#include "move_base_msgs/MoveBaseActionGoal.h"
#include "move_base_msgs/MoveBaseGoal.h"
#include "std_msgs/Int8.h"
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

void taskGoalCallback(const std_msgs::Int8::ConstPtr &msg)
{
    ROS_INFO("Got task goal!");
    static MoveBaseClient ac("move_base", true);
    bool first_in = true;
    if (first_in)
        while (!ac.waitForServer(ros::Duration(5.0)))
        {
            ROS_INFO("Waiting for the move_base action server to come up");
        }
    ros::NodeHandle nh2;
    ros::Publisher pub = nh2.advertise<move_base_msgs::MoveBaseActionGoal>("/move_base/goal", 10);

    // 在这里处理接收到的任务目标消息
    int taskGoal = msg->data;
    // 进行相应的操作或逻辑处理
    ROS_INFO("%d", taskGoal);

    // 构造目标点消息
    move_base_msgs::MoveBaseActionGoal target;
    move_base_msgs::MoveBaseGoal goal;
    // 消息头
    target.header.frame_id = "map";

    // 设置目标位置和方向信息
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    if (taskGoal == 0)
    {
        goal.target_pose.pose.position.x = 0.1;  // X
        goal.target_pose.pose.position.y = 0.08; // Y
        goal.target_pose.pose.position.z = 0;

        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z = 0;
        goal.target_pose.pose.orientation.w = -0.12; // yaw
    }
    else if (taskGoal == 1)
    {
        goal.target_pose.pose.position.x = 0.21;  // X
        goal.target_pose.pose.position.y = -1.36; // Y
        goal.target_pose.pose.position.z = 0;

        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z = 0;
        goal.target_pose.pose.orientation.w = 177.3; // yaw
    }
    else if (taskGoal == 2)
    {
        goal.target_pose.pose.position.x = 4.11; // X
        goal.target_pose.pose.position.y = -2.16; // Y
        goal.target_pose.pose.position.z = 0;

        goal.target_pose.pose.orientation.x = 0.0;
        goal.target_pose.pose.orientation.y = 0.0;
        goal.target_pose.pose.orientation.z = 0;
        goal.target_pose.pose.orientation.w = 178.95; // yaw
    }

    target.goal = goal;
    // 发送目标点
    ROS_INFO("Goal sent");
    ac.sendGoal(goal);
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        ROS_INFO("Goal reached");
    else
        ROS_ERROR("Navigation failed for some reason");

    // pub.publish(target);
    // pub.publish(target);
    // pub.publish(target);
    // pub.publish(target);
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "goal_setter");

    ros::NodeHandle nh;

    // 创建一个订阅者，订阅名为"/task_goal"的主题
    ros::Subscriber sub = nh.subscribe("/task_goal", 1, taskGoalCallback);
    sleep(2);

    // 循环等待回调函数
    ros::spin();

    return 0;
}
